#include <robot_chassis/CRobotControl.hpp>

CRobotControl::CRobotControl(const std::string& nodeName)
    : Node(nodeName), turnonRobot_(CTurnOnRobot::getInstance())
{
    startReceive();
    RCLCPP_INFO(this->get_logger(), "速度控制节点已启动 - 订阅话题: /chassis/cmd_vel");
}

CRobotControl::~CRobotControl() {}

void CRobotControl::startReceive()
{
    sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "chassis/cmd_vel", 10, std::bind(&CRobotControl::callback, this, std::placeholders::_1));
}

void CRobotControl::callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    setRobotVel_.x_ = static_cast<float>(msg->linear.x);
    setRobotVel_.y_ = static_cast<float>(msg->linear.y);
    setRobotVel_.w_ = static_cast<float>(msg->angular.z);
    turnonRobot_.setRobotVel(setRobotVel_);
}
